#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <list>

#include "Meshes/StructuredMesh.hpp"
#include "Meshes/Uniform.hpp"
#include "Utils/inout.hpp" 

#include "ParTrack/ParticleTracking.hpp"
#include "ParTrack/Linear.hpp"
#include "ParTrack/RungeKutta4.hpp"
#define PI 3.14159265

using namespace std;
using namespace Tuna;

typedef TunaArray<double, 2>::tiny floatTinyArray_t;
typedef TunaArray<double, 2>::huge ScalarField2D;

void writeTofile_DX(list<floatTinyArray_t> &, int, string);
inline double dot(floatTinyArray_t &, floatTinyArray_t &);
void makeCircle(double, double, double, list<floatTinyArray_t> &, int);
void reducePoints(list<floatTinyArray_t> &, double);
void trackCircle(list<floatTinyArray_t> &, double);

StructuredMesh< Uniform<double, 2> > mesh(1, 257, 1, 257);
floatTinyArray_t deltas = mesh.getDeltas();
double dt;
ScalarField2D u(mesh.getExtentNodes());       
ScalarField2D v(mesh.getExtentNodes());
ScalarField2D un(mesh.getExtentNodes());       
ScalarField2D vn(mesh.getExtentNodes());

ParticleTracking< RungeKutta4< Linear<double, 2> > > particle;

int main () 
{

    double tol, tolf;
    list<floatTinyArray_t> circle1, circle2, circle3;

    ofstream flong1("longcirc1"), flong2("longcirc2"), flong3("longcirc3");

    int Npoints, Nsteps, frec, Ncicles, reduce ;
    cout << " Npoints = "; cin >> Npoints;
    cout << " Steps = "; cin >> Nsteps;
    cout << " Time step = "; cin >> dt;
    cout << " Tolerance = "; cin >> tol;
    cout << " Fold = "; cin >> tolf;
    cout << " No Cicles = "; cin >> Ncicles;
    cout << " Print freq = "; cin >> frec;
    cout << " Freq. reduction = "; cin >> reduce;

    makeCircle(0.5, 0.75, 0.125, circle1, Npoints);
    makeCircle(0.5, 0.5, 0.125, circle2, Npoints);
    makeCircle(0.5, 0.25, 0.125, circle3, Npoints);
    writeTofile_DX(circle1, 0, "circle1.");
    writeTofile_DX(circle2, 0, "circle2.");
    writeTofile_DX(circle3, 0, "circle3.");

    cout << "\n ----- Begining track ... \n\n";


///
/// Follow all the particles for several cicles...
///
    int count = 1;
    for(int cicles = 1; cicles <= Ncicles; ++cicles) {
	cout << " ----> Cicle " << cicles << endl;
	for(int step = 1; step <= Nsteps; ++step) {
	    InOut::readFromFile_1(u, v, step, "velc.");
	    InOut::readFromFile_1(un, vn, step + 1, "velc.");

	    trackCircle(circle1, tol);
	    trackCircle(circle2, tol);
	    trackCircle(circle3, tol);

	    if( !(count % reduce) ) {
		reducePoints(circle1, tolf);
		reducePoints(circle2, tolf);
		reducePoints(circle3, tolf);
	    }
	    
	    if( !(count % frec) ) {
		writeTofile_DX(circle1, count, "circle1.");
		writeTofile_DX(circle2, count, "circle2.");
		writeTofile_DX(circle3, count, "circle3.");
	    }

	    flong1 << count << "\t" << circle1.size() << endl;
	    flong2 << count << "\t" << circle2.size() << endl;
	    flong3 << count << "\t" << circle3.size() << endl;
	    
	    cout << " Circle 1: step = " << step 
		 << " count = " << count 
		 << " Npoints = " << circle1.size()
		 << endl;
	    cout << " Circle 2: step = " << step 
		 << " count = " << count 
		 << " Npoints = " << circle2.size()
		 << endl;
	    cout << " Circle 3: step = " << step 
		 << " count = " << count 
		 << " Npoints = " << circle3.size()
		 << endl;


	    count++;
	}


    }
    
    cout << "\n End ... \n\n";

    flong1.close();
    flong2.close();
    flong3.close();

    return 0;
}

void makeCircle(double xc, double yc, double r, 
		list<floatTinyArray_t> &circle, int N) 
{
    floatTinyArray_t aux;
    double theta = 2 * PI / N;
    for(int i = 0; i < N; i++) {
	aux(0) = xc + r * cos (theta * i);
	aux(1) = yc + r * sin (theta * i);
	circle.push_back(aux);
    }
///
/// Add the first point to the end of the list to close the circle.
///
    aux(0) = xc + r;
    aux(1) = yc;
    circle.push_back(aux);
}


void trackCircle(list<floatTinyArray_t> &circle, double tol) {
///
/// Track every particle on the circle
///
    double dist;
    list<floatTinyArray_t>::iterator iter, iter_p1;
    floatTinyArray_t aux, x_i_n, x_i_np1, x_im1_n, x_im1_np1;
///
/// Initial point in the line  x(i-1)^n
///
    iter = circle.begin(); 
    x_im1_n = *iter;      
/// Track first point to the next position x(i-1)^(n+1) 
    *iter = particle.track(*iter, deltas, u, v, un, vn, dt);
    x_im1_np1 = *iter; 
/// Advance iter to the second point in the line
    iter++;
    for( ; iter != circle.end(); ++iter) {
/// Current point  x(i)^n
	x_i_n = *iter; 
/// Track the current point to the next position, x(i)^(n+1)
	x_i_np1 = particle.track(*iter, deltas, u, v, un, vn, dt);
/// Calculate the distance between consecutive tracked points 
/// x(i-1)^(n+1) and x(i)^(n+1)
	dist = sqrt( ( x_i_np1(0) - x_im1_np1(0) ) * 
		     ( x_i_np1(0) - x_im1_np1(0) ) +
		     ( x_i_np1(1) - x_im1_np1(1) ) * 
		     ( x_i_np1(1) - x_im1_np1(1) ) );
/// if dist exceeds the tolerance, adds a point between points: 
/// x(i)^n and x(i-1)^n
	if ( dist > tol ) {
	    aux(0) = ( x_i_n(0) + x_im1_n(0) ) * 0.5;
	    aux(1) = ( x_i_n(1) + x_im1_n(1) ) * 0.5;
	    circle.insert(iter,aux);
/// Rewind the iterator 2 positions, because the for will advance one
/// position.
	    iter--; iter--;
	} else {
/// If dist does not exceeds the tolerance, then re-state the value
/// of x(i-1)^n, x(i-1)^(n+1) and modify the new position of x(i) in the
/// list ( x(i)^(n+1) )
	    x_im1_n   = x_i_n;
	    x_im1_np1 = x_i_np1;
	    *iter     = x_i_np1;
	}
    }
}

void reducePoints(list<floatTinyArray_t> &circle, double tolf) 
{
    double fold;
    floatTinyArray_t x_im1, x_ip1, x_i, r_ip1, r_im1;
    list<floatTinyArray_t>::iterator iter = circle.begin(), iter_p1;
    x_im1 = *iter;
    iter++;
    for( ; iter != --circle.end(); ++iter) {
	x_i = *iter;
	iter_p1 = iter;
	x_ip1 = *(++iter_p1);
	r_ip1(0) = x_i(0) - x_ip1(0);
	r_ip1(1) = x_i(1) - x_ip1(1);
	r_im1(0) = x_i(0) - x_im1(0);
	r_im1(1) = x_i(1) - x_im1(1);
	fold = 0.5 * dot(r_im1, r_ip1) +  0.5;
	if ( fold <= tolf ) {
	    iter_p1 = iter;
	    iter--;
	    circle.erase(iter_p1);
	} else {
	    x_im1 = x_i;
	}
    }
}

inline double dot(floatTinyArray_t &r_im1, floatTinyArray_t &r_ip1) 
{
    double mm1 = sqrt( r_im1(0) * r_im1(0) + r_im1(1) * r_im1(1) );
    double mp1 = sqrt( r_ip1(0) * r_ip1(0) + r_ip1(1) * r_ip1(1) );
    return ( (r_im1(0) * r_ip1(0) + r_im1(1) * r_ip1(1)) / ( mm1 * mp1 ) );
}


void writeTofile_DX(list<floatTinyArray_t> &circle, int n, string filename)
{

    int Npoints = circle.size();
    list<floatTinyArray_t>::iterator iter;

    ostringstream extnumber;
    extnumber.width(5);
    extnumber.fill('0');
    extnumber << n;
    string snumber = extnumber.str();
    filename += snumber;
    ofstream file (filename.c_str());  

    cout << "writing " << filename << endl;
    
    file << " object \"position list\" class array type float rank 1 shape 2 items "
	 << Npoints << " data follows\n";
    for(iter = circle.begin(); iter != circle.end(); ++iter)
	file << (*iter)(0) << "   " << (*iter)(1) << "   ";
    file << endl;

    file << "object \"edge list\" class array type int rank 0 items "
	 << Npoints - 1 << " data follows\n";
    for(int i = 0; i < Npoints - 1; ++i)
	file << i << "   ";
    file << endl;
    file << "attribute \"ref\" string \"positions\"\n";

    file << "object \"loop list\" class array type int rank 0 items "
	 << 1 << " data follows\n";
    file << 0 << endl;
    file << "attribute \"ref\" string \"edges\"\n";

    file << "object \"face list\" class array type int rank 0 items "
	 << 1 << " data follows\n";
    file << 0 << endl;
    file << "attribute \"ref\" string \"loops\"\n";

    file << "object \"data\" class array type float rank 0 items "
	 << 1 << " data follows\n";
    file << 1.9 << endl;
    file << "attribute \"dep\" string \"faces\"\n";

    file << "object \"map\" class field\n"
	 << "component \"positions\" \"position list\" \n"
	 << "component \"edges\" \"edge list\" \n"
	 << "component \"loops\" \"loop list\" \n"
	 << "component \"faces\" \"face list\" \n"
	 << "component \"data\" \"data\" \n"
	 << "end\n" ;
    file.close();

}


